/*************************************************************************
*
*	waypoints.c
*
*  Copyright 2007 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*  This module contains functions to deal with the waypoint structure.
*  
*
*  Revision 1.01, 10/3/07, SBS
*    - minor cleanup
*
*  Revision 1.0, 6/7/07, SBS
*    - separated from status.c and irc_client.c
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/

#include <stdio.h>
#include <stdlib.h>
//#include <time.h>
#include <string.h>

#include <libgnome/gnome-config.h>

#include "waypoints.h"
#include "readconfigfile.h"
//#include "status.h"

#define CMDSTRLENGTH 512

#define INITVAL -255

// max number of waypoints to consider
#define MAXWPT 16	


/************************************************
*
*  waypoints_InitPosition()
*
*  returns an initialized position struct
*
************************************************/
POSITION waypoints_InitPosition()
{
	POSITION p;
	
	p.dLatDeg = INITVAL;
	p.dLonDeg = INITVAL;
	
	return(p);
}	

/************************************************
*
* waypoints_PrintWpt()
*
* Prints a waypoint to the screen
*
* Parameters:
*    wpt - list of waypoints in the current path
*    num - index to the waypoint being printed
*
* Returns:
*   none
*
************************************************/
void waypoints_PrintWpt(WAYPT wpt,int num)
{
	printf("=====SBS===== wpts[%d].Lat   = %f\n",num,wpt.dLatDeg);
	printf("=====SBS===== wpts[%d].Lon   = %f\n",num,wpt.dLonDeg);
	printf("=====SBS===== wpts[%d].Speed = %f\n",num,wpt.dSpeedMps);
	fflush(stdout);
	return;
}
/************************************************
*
*  waypoints_PrintWaypoints()
*
*  Prints a list of waypoints to the screen
*
*  Parameters:
*    wp - array of waypoints
*    count - number of waypoints
*
************************************************/
void waypoints_PrintWaypoints(WAYPT *wp, int count)
{
	register int i,j;

	if(count > MAXWPT){
		j = MAXWPT;
	}else{
		j = count;
	}
	
	for(i=0; i<j; i++){
		printf("===SBS=== Waypoint #%d = (%f,%f)\n"
				, i
				, wp[i].dLatDeg
				, wp[i].dLonDeg);
	
	}

	fflush(stdout);

	return;
}
	

/************************************************
*
* waypoints_ReadFile()
*
* Reads waypoints from a file 
*
* Parameters:
*   filespec - the filename
*   wpcount - number of waypoints
*   wp - array of waypoints to fill
*
* Returns: 
*    none
*
************************************************/
void waypoints_ReadFile(char *filespec, int wpcount, WAYPT *wp)
{

	char cmd[CMDSTRLENGTH];
	register int i,j;

	if(wpcount > MAXWPT){
		j = MAXWPT;
	}else{
		j = wpcount;
	}
	
	for(i=0; i<j; i++){
	
		snprintf(cmd,CMDSTRLENGTH,"waypoint%d_lat",i);
		wp[i].dLatDeg = readconfig_float(filespec,"path",cmd,0);
		snprintf(cmd,CMDSTRLENGTH,"waypoint%d_lon",i);
		wp[i].dLonDeg = readconfig_float(filespec,"path",cmd,0);
	
	}				

	return;
}

/************************************************
*
* waypoints_WriteFile()
*
************************************************/
void waypoints_WriteFile(char *filespec, int count, WAYPT *wp)
{
	register int i,j;
	char cmd[CMDSTRLENGTH];
	
	char prefix[CMDSTRLENGTH];
	
	snprintf(prefix,CMDSTRLENGTH,"=%s=/",filespec);

	gnome_config_push_prefix(prefix);

	if(count > MAXWPT){	//only use first MAXWPT waypoints
		j = MAXWPT;
	}else{
		j = count;
	}
	
	for (i=0; i<j; i++){		
		snprintf(cmd,CMDSTRLENGTH,"path/waypoint%d_lat",i);	
		gnome_config_set_float(cmd, wp[i].dLatDeg);

		snprintf(cmd,CMDSTRLENGTH,"path/waypoint%d_lon",i);	
		gnome_config_set_float(cmd, wp[i].dLonDeg);
	}

	gnome_config_pop_prefix();
	
	gnome_config_sync();
	return;
}

